#include "ros/ros.h"
// #include <pcl/point_cloud.h>
// #include <pcl_conversions/pcl_conversions.h>
// #include <pcl/segmentation/extract_clusters.h>
// #include <pcl/point_types.h>
// #include <pcl/PointIndices.h>
// #include <pcl/io/io.h>
// #include <pcl/io/pcd_io.h>
// #include <sensor_msgs/PointCloud2.h>
// #include <pcl/common/transforms.h>
// #include <pcl/filters/voxel_grid.h>
// #include <pcl/filters/passthrough.h>
// #include <visualization_msgs/Marker.h>
// #include <visualization_msgs/MarkerArray.h>
// #include <geometry_msgs/PointStamped.h>
#include "gmm_test/gmm_test.h"
#include "gmm_test/gmm_sample.h"

ros::Subscriber cloud_sub;
ros::Publisher ellipsoid_marker_array_pub;
ros::Publisher cluster_centre_pub;
ros::Publisher cloud_pub;
ros::Publisher point_pub;
int AGENT_ID = 0;

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud);
void visual_ellipsoid(std::vector<Eigen::Vector3d> means,std::vector<Eigen::Matrix3d> var,double n_sigma,int id);

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //执行节点初始化
    ros::init(argc,argv,"GMM_test");
	ros::NodeHandle nh("~");

	point_pub = nh.advertise<sensor_msgs::PointCloud2>("gmm_cloud_sample",1);
    cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("gmm_cloud",1);
    ellipsoid_marker_array_pub = nh.advertise<visualization_msgs::MarkerArray>("GMM_visual_1",1);
    cluster_centre_pub = nh.advertise<visualization_msgs::Marker>("GMM_visual_2",1);
    cloud_sub=nh.subscribe("/iris_0/camera/depth/points", 1, cloudCallback);
	
    ros::Duration(0.1).sleep();
	ros::spin();
    return 0;
}


void cloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    ros::Time cloudcbstart=ros::Time::now();

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*cloud, *cloud_in);

    if(cloud_in->points.size()<=20) return;

    //////////////////////////////////////////////STEP1//////////////////////////////////////////////
    //////////////////////////////////////////////直通滤波//////////////////////////////////////////////
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_1(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PassThrough<pcl::PointXYZ> pass;
	pass.setInputCloud (cloud_in);
	pass.setFilterFieldName ("z");// 滤波字段设置为z轴方向
	pass.setFilterLimits (1, 4.3);
	//pass.setFilterLimitsNegative (true); //默认为flase。true为过滤掉范围内的，flase为过滤掉范围外的
	pass.filter (*cloud_1);

    pass.setInputCloud (cloud_1);
    pass.setFilterFieldName ("y");
    pass.setFilterLimits (-5, 0.1);
    pass.filter (*cloud_1);


    //////////////////////////////////////////////STEP2//////////////////////////////////////////////
    //////////////////////////////////////////////体素滤波//////////////////////////////////////////////
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::VoxelGrid<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud_1);
	static float res = 0.1;
	sor.setLeafSize(res, res, res);
	sor.filter(*cloud_2);

    //////////////////////////////////////////////STEP3//////////////////////////////////////////////
    //////////////////////////////////////////////点云格式转换//////////////////////////////////////////////

    if(cloud_2->points.size() < 20) return;

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudNotAtedge(new pcl::PointCloud<pcl::PointXYZRGB>());
	for(int i=0;i<(int)cloud_2->points.size();i++)
	{
		Eigen::Vector3d tmp_point(cloud_2->points[i].x,cloud_2->points[i].y,cloud_2->points[i].z);
		pcl::PointXYZRGB point;
		point.x=cloud_2->points[i].x;
		point.y=cloud_2->points[i].y;
		point.z=cloud_2->points[i].z;
		point.b=255;
		cloudNotAtedge->points.push_back(point);
	}

    ros::Time cloudmid=ros::Time::now();

    //////////////////////////////////////////////STEP4//////////////////////////////////////////////
    //////////////////////////////////////////////欧式聚类//////////////////////////////////////////////
	/// 创建kd树
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
	tree->setInputCloud(cloudNotAtedge);
	/// 设置分割参数, 执行欧式聚类分割
	std::vector<pcl::PointIndices> cluster_indices;
	pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
	ec.setClusterTolerance(0.3);	//设置近邻搜索的半径
	ec.setMinClusterSize(1);		//设置最小聚类点数
	ec.setMaxClusterSize(9999);	//设置最大聚类点数
	ec.setSearchMethod(tree);
	ec.setInputCloud(cloudNotAtedge);
	ec.extract(cluster_indices);

    if(cluster_indices.size()==0)return;


    //////////////////////////////////////////////STEP5//////////////////////////////////////////////
    ////////////////////////////聚类算法（K-means与EM的分层拟合）///////////////////////////
	std::vector<Eigen::Vector3d> means;
    std::vector<Eigen::Matrix3d> var;
    std::vector<double> pi_vector;

	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
	{
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
		std::vector<Eigen::Vector3d> point_cloud;
		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
		{
			Eigen::Vector3d	tmp(cloudNotAtedge->points[*pit].x,cloudNotAtedge->points[*pit].y,cloudNotAtedge->points[*pit].z);
			point_cloud.push_back(tmp);
		}
		gy::GMM_fit GMM;
		GMM.point = point_cloud;
		GMM.GMM_fit_layer();
		for(int i = 0;i<GMM.miu.size();i++){
			means.push_back(GMM.miu[i]);
			var.push_back(GMM.sigma[i]);
			pi_vector.push_back(1.0/(double)GMM.miu.size());
		}
	}


    ros::Time cloudend =ros::Time::now();
    ROS_INFO("Tatal using time = %f, Fitting using time %f.",(cloudend-cloudcbstart).toSec(),(cloudend-cloudmid).toSec());
    //////////////////////////////////////////////STEP6//////////////////////////////////////////////
    /////////////////////////////////////////////拟合结果可视化///////////////////////////////////////
    if(means.size() == 0)return;
	for(int  i = 0;i<means.size();i++){
		means[i](1) += 3;
	}

    sensor_msgs::PointCloud2 cloud2_pub;
	for(int i = 0;i<cloud_2->points.size();i++){
		cloud_2->points[i].y += 3.0;
	}
	pcl::toROSMsg(*cloud_2,cloud2_pub);//滤波后的点云转换为ROS消息
	cloud2_pub.header.frame_id = "iris_0/camera_link";

    visual_ellipsoid(means,var,2.0,0);
    cloud_pub.publish(cloud2_pub);

    //////////////////////////////////////////////STEP7//////////////////////////////////////////////
    /////////////////////////////////////////////重构点云及可视化//////////////////////////////////

	std::vector<Eigen::Vector3d> point_sample;
	pcl::PointCloud<pcl::PointXYZ>::Ptr gvmcbMap_pc(new pcl::PointCloud<pcl::PointXYZ>());
	
	gvm::GMM gmm_sample;
	gmm_sample.init(pi_vector,means,var,means.size(),means[0]);
	point_sample = gmm_sample.GMMRandomSamplePoints(3000,2.5);

	//point_sample = point_sample_(means,var,pi_vector,8000,1.6);

	for(int i = 0;i<point_sample.size();i++){
		pcl::PointXYZ vis_point;
		vis_point.x = point_sample[i](0);
		vis_point.y = point_sample[i](1)+3.5;
		vis_point.z = point_sample[i](2);
		gvmcbMap_pc->points.push_back(vis_point);
	}
	sensor_msgs::PointCloud2 gvmcbMap_ros;
	pcl::toROSMsg(*gvmcbMap_pc, gvmcbMap_ros);
	gvmcbMap_ros.header.stamp = ros::Time::now();
	gvmcbMap_ros.header.frame_id = "iris_0/camera_link";
	point_pub.publish(gvmcbMap_ros);
	std::cout<<point_sample.size()<<std::endl;

}


void visual_ellipsoid(std::vector<Eigen::Vector3d> means,std::vector<Eigen::Matrix3d> var,double n_sigma,int id)
{

visualization_msgs::MarkerArray marker_array;

int cluster_num=means.size();
// ROS_INFO("means var size %ld %ld",means.size(),var.size());

for(int i=0;i<cluster_num;i++)
{
for(int j=0;j<3;j++)
{
for(int k=0;k<3;k++)
{
if(fabs(var[i](j,k))<1e-5)
{
var[i](j,k)=0;
}
}
}

Eigen::EigenSolver<Eigen::Matrix3d> es(var[i]);

Eigen::Matrix3d value_martrix = es.pseudoEigenvalueMatrix();
Eigen::Matrix3d vector_martrix = es.pseudoEigenvectors();

vector_martrix.col(2)=vector_martrix.col(0).cross(vector_martrix.col(1));
// std::cout<<"var: "<<var[i]<<endl;

// std::cout<<"value_martrix: \n"<<value_martrix<<endl;
// std::cout<<"vector_martrix: \n"<<vector_martrix<<endl;
// std::cout<<"means[i]: "<<means[i]<<endl;


double var0=pow(value_martrix(0,0),0.5)*n_sigma;
double var1=pow(value_martrix(1,1),0.5)*n_sigma;
double var2=pow(value_martrix(2,2),0.5)*n_sigma;
// ROS_INFO("i: %d var: %f %f %f, means: %f %f %f",i,var0,var1,var2,means[i](0),means[i](1),means[i](2));

visualization_msgs::Marker marker;

marker.header.frame_id = "iris_0/camera_link";
marker.header.stamp = ros::Time();
marker.id = 23123+i+id+AGENT_ID;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;

marker.lifetime = ros::Duration(100.0/30.0*1.1);

marker.pose.position.x = means[i](0);
marker.pose.position.y = means[i](1);
marker.pose.position.z = means[i](2);

Eigen::Quaterniond quaternion(vector_martrix);
quaternion.normalize();
marker.pose.orientation.x = quaternion.x();
marker.pose.orientation.y = quaternion.y();
marker.pose.orientation.z = quaternion.z();
marker.pose.orientation.w = quaternion.w();

marker.scale.x = var0*2;
marker.scale.y = var1*2;
marker.scale.z = var2*2;

marker.color.a = 0.5; // Don't forget to set the alpha!
marker.color.r = 0.0;
marker.color.g = 0.0;
marker.color.b = 0.0;

marker_array.markers.push_back(marker);

}

ellipsoid_marker_array_pub.publish(marker_array);

// visulization of cluster centre
visualization_msgs::Marker point_marker;

point_marker.id = 333;
point_marker.header.frame_id = "iris_0/camera_link";
point_marker.header.stamp = ros::Time::now();
point_marker.type = visualization_msgs::Marker::SPHERE_LIST;
point_marker.action = visualization_msgs::Marker::ADD;
point_marker.ns = "lines_and_points";

point_marker.scale.x = 0.1;
point_marker.scale.y = 0.1;
point_marker.scale.z = 0.1;

point_marker.lifetime = ros::Duration(100);

point_marker.color.r=1;
point_marker.color.g=0;
point_marker.color.b=0;
point_marker.color.a=1.0;

for(int i=0;i<means.size();i++)
{
geometry_msgs::Point point;
point.x = means[i](0);
point.y = means[i](1);
point.z = means[i](2);
point_marker.points.push_back(point);
}

cluster_centre_pub.publish(point_marker);

}